/*
Copyright (C) 2011 by the PSVN Research Group, University of Alberta
*/

#include <vector>
#include "priority_queue.hpp"

using namespace std;

int main( int argc, char **argv )
{
    int64_t totalNodes, depthSum, numAtD;
    state_t state, child;
    PriorityQueue<state_t> open;
    int d, rule_used;
    func_ptr iter;
    state_map_t *map = new_state_map();
    FILE *file;

    if( argc < 2 ) {
        fprintf( stderr, "usage: bfs output_file\n" );
        exit( -1 );
    }
    
    /* add goal states */
    totalNodes = 0;
    depthSum = 0;
    first_goal_state( &state, &d ); do {
        state_map_add( map, &state, 0 );
        open.Add(0, 0, state );
    } while( next_goal_state( &state, &d ) );

    d = 0;
    numAtD = 0;
    while( !open.Empty() ) {

        /* get current distance from goal; since operator costs are
           non-negative this distance is monotonically increasing */
        if (open.CurrentPriority() > d) {
            printf( "%zu states at distance %d\n", numAtD, d );
            d = open.CurrentPriority();
            numAtD = 0;
        }

        /* get state */
        state = open.Top();
        open.Pop();
        
        /* check for stale arrival: ie, we already expanded this state. */
        const int *best_dist = state_map_get( map, &state );
        assert(best_dist != NULL);
        if (*best_dist < d)
            continue;
        
        numAtD++;
        totalNodes++;
        depthSum += d;

        /* look at all children */
        init_bwd_iter( iter );
        while( ( rule_used = next_bwd_iter( iter, &state ) ) >= 0 ) {
            apply_bwd_rule( rule_used, &state, &child );
            const int child_d = d + bwd_rule_costs[ rule_used ];

            /* check if either this child has not been seen yet or if
               there is a new cheaper way to get to this child. */
            const int *old_child_d = state_map_get( map, &child );
            if ( old_child_d == NULL || *old_child_d > child_d ) {
                /* relax distance and add to open */
                state_map_add( map, &child, child_d );
                open.Add( child_d, child_d, child );
            }
        }
    }
    
    double avg = (double)depthSum / (double)totalNodes;
    printf( "%"PRIu64" states, average distance of %lf\n", totalNodes, avg );
    
    /* save the map */
    file = fopen( argv[ 1 ], "w" );
    if( file == NULL ) {
        fprintf( stderr, "could not open %s for writing\n", argv[ 1 ] );
        exit( -1 );
    }
    write_state_map( file, map );
    fclose( file );
    
    return 0;
}
